// --------------------------------------------------------------------------------------------------------------------
// <copyright file="RegionCheckerTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Numerics
{
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// test the region checker
    /// </summary>
    [TestClass]
    public class RegionCheckerTests
    {
        /// <summary>
        /// build a simple work space by providing several inaccessible regions and test
        /// the region checker
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RegionCheckerUsingBoxRegionsTest()
        {
            RegionAccessibilityChecker rc = new RegionAccessibilityChecker();

            // add a very thin but long stick
            // center it at [5,5,5]
            rc.AddInaccessibleRegion(new BoxRegion(0.2, 0.2, 5, new Pose(new Vector3(5, 5, 5), new Quaternion(0, 0, 0, 1))));

            // add another very thin but long stick
            // center it at [-5,-5,-5], and rotate it around y axis for 90 degrese
            rc.AddInaccessibleRegion(new BoxRegion(5, 0.2, 0.2, new Pose(new Vector3(-5, -5, -5), new Quaternion(0, 0.7071, 0, 0.7071))));

            Assert.IsTrue(rc.IsSafe(new Vector3(5, 5, 8)));
            Assert.IsFalse(rc.IsSafe(new Vector3(5, 5, 7.4)));
            Assert.IsFalse(rc.IsSafe(new Vector3(5, 5, 5)));

            Assert.IsFalse(rc.IsSafe(new Vector3(-5, -5, -5)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5, -4, -5)));
            Assert.IsFalse(rc.IsSafe(new Vector3(-5.09, -5.09, -5.09)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.11, -5.09, -5.09)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.09, -5.11, -5.09)));
            Assert.IsFalse(rc.IsSafe(new Vector3(-5.09, -5.09, -5.11)));
            Assert.IsFalse(rc.IsSafe(new Vector3(-5, -5, -4.95)));
            Assert.IsFalse(rc.IsSafe(new Vector3(-5, -5, -4)));

            // clear the inaccessible regions
            rc.ClearAllInaccessibleRegions();

            // now they should be all safe
            Assert.IsTrue(rc.IsSafe(new Vector3(5, 5, 8)));
            Assert.IsTrue(rc.IsSafe(new Vector3(5, 5, 7.4)));
            
            Assert.IsTrue(rc.IsSafe(new Vector3(-5, -5, -5)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5, -4, -5)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.09, -5.09, -5.09)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.11, -5.09, -5.09)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.09, -5.11, -5.09)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.09, -5.09, -5.11)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5, -5, -4.95)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5, -5, -4)));
        }

        /// <summary>
        /// build a simple work space by providing several inaccessible regions and test
        /// the region checker
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RegionCheckerUsingBoxRegionsAndNonDefaultConstructorTest()
        {
            List<BoxRegion> boxes = new List<BoxRegion>();

            // add a very thin but long stick
            // center it at [5,5,5]
            boxes.Add(new BoxRegion(0.2, 0.2, 5, new Pose(new Vector3(5, 5, 5), new Quaternion(0, 0, 0, 1))));

            // add another very thin but long stick
            // center it at [-5,-5,-5], and rotate it around y axis for 90 degrese
            boxes.Add(new BoxRegion(5, 0.2, 0.2, new Pose(new Vector3(-5, -5, -5), new Quaternion(0, 0.7071, 0, 0.7071))));

            RegionAccessibilityChecker rc = new RegionAccessibilityChecker(boxes, null);

            Assert.IsTrue(rc.IsSafe(new Vector3(5, 5, 8)));
            Assert.IsFalse(rc.IsSafe(new Vector3(5, 5, 7.4)));
            Assert.IsFalse(rc.IsSafe(new Vector3(5, 5, 5)));

            Assert.IsFalse(rc.IsSafe(new Vector3(-5, -5, -5)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5, -4, -5)));
            Assert.IsFalse(rc.IsSafe(new Vector3(-5.09, -5.09, -5.09)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.11, -5.09, -5.09)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.09, -5.11, -5.09)));
            Assert.IsFalse(rc.IsSafe(new Vector3(-5.09, -5.09, -5.11)));
            Assert.IsFalse(rc.IsSafe(new Vector3(-5, -5, -4.95)));
            Assert.IsFalse(rc.IsSafe(new Vector3(-5, -5, -4)));

            // clear the inaccessible regions
            rc.ClearAllInaccessibleRegions();

            // now they should be all safe
            Assert.IsTrue(rc.IsSafe(new Vector3(5, 5, 8)));
            Assert.IsTrue(rc.IsSafe(new Vector3(5, 5, 7.4)));

            Assert.IsTrue(rc.IsSafe(new Vector3(-5, -5, -5)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5, -4, -5)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.09, -5.09, -5.09)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.11, -5.09, -5.09)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.09, -5.11, -5.09)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.09, -5.09, -5.11)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5, -5, -4.95)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5, -5, -4)));
        }

        /// <summary>
        /// build a simple work space by providing several inaccessible regions and test
        /// the region checker
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RegionCheckerUsingHalfSpaceRegionsTest()
        {
            RegionAccessibilityChecker rc = new RegionAccessibilityChecker();
         
            // add a separating plane passing (0,0,-5) with its normal
            // aligning with the z-axis
            rc.AddInaccessibleRegion(new HalfSpaceRegion(new Vector3(0, 0, 1), new Vector3(0, 0, -5)));

            // add a separating plane passing (-5,0,0) with its normal
            // aligning with the x-axis
            rc.AddInaccessibleRegion(new HalfSpaceRegion(new Vector3(1, 0, 0), new Vector3(-5, 0, 0)));

            Assert.IsTrue(rc.IsSafe(new Vector3(0, 0, -4.9)));
            Assert.IsFalse(rc.IsSafe(new Vector3(0, 0, -5.1)));

            Assert.IsFalse(rc.IsSafe(new Vector3(-5.1, 0, 0)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-4.9, 0, 0)));

            // clear all the inaccessible regions
            rc.ClearAllInaccessibleRegions();

            // now they should be all safe
            Assert.IsTrue(rc.IsSafe(new Vector3(0, 0, -4.9)));
            Assert.IsTrue(rc.IsSafe(new Vector3(0, 0, -5.1)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.1, 0, 0)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-4.9, 0, 0)));
        }

        /// <summary>
        /// build a simple work space by providing several inaccessible regions and test
        /// the region checker
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RegionCheckerUsingHalfSpaceRegionsAndNonDefaultConstructorTest()
        {
            List<HalfSpaceRegion> halfspaces = new List<HalfSpaceRegion>();

            // add a separating plane passing (0,0,-5) with its normal
            // aligning with the z-axis
            halfspaces.Add(new HalfSpaceRegion(new Vector3(0, 0, 1), new Vector3(0, 0, -5)));

            // add a separating plane passing (-5,0,0) with its normal
            // aligning with the x-axis
            halfspaces.Add(new HalfSpaceRegion(new Vector3(1, 0, 0), new Vector3(-5, 0, 0)));

            RegionAccessibilityChecker rc = new RegionAccessibilityChecker(null, halfspaces);

            Assert.IsTrue(rc.IsSafe(new Vector3(0, 0, -4.9)));
            Assert.IsFalse(rc.IsSafe(new Vector3(0, 0, -5.1)));

            Assert.IsFalse(rc.IsSafe(new Vector3(-5.1, 0, 0)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-4.9, 0, 0)));

            // clear all the inaccessible regions
            rc.ClearAllInaccessibleRegions();

            // now they should be all safe
            Assert.IsTrue(rc.IsSafe(new Vector3(0, 0, -4.9)));
            Assert.IsTrue(rc.IsSafe(new Vector3(0, 0, -5.1)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-5.1, 0, 0)));
            Assert.IsTrue(rc.IsSafe(new Vector3(-4.9, 0, 0)));
        }

        /// <summary>
        /// check a box region with invalid length
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(System.ArgumentException))]
        public void RegionCheckerUsingBoxRegionInvalidLengthTest()
        {
            RegionAccessibilityChecker rc = new RegionAccessibilityChecker();

            // add an invalid box region
            rc.AddInaccessibleRegion(new BoxRegion(-1, 1, 1, Pose.Identity));
        }

        /// <summary>
        /// check a box region with invalid width
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(System.ArgumentException))]
        public void RegionCheckerUsingBoxRegionInvalidWidthTest()
        {
            RegionAccessibilityChecker rc = new RegionAccessibilityChecker();

            // add an invalid box region
            rc.AddInaccessibleRegion(new BoxRegion(1, -1, 1, Pose.Identity));
        }

        /// <summary>
        /// check a box region with invalid height
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(System.ArgumentException))]
        public void RegionCheckerUsingBoxRegionInvalidHeightTest()
        {
            RegionAccessibilityChecker rc = new RegionAccessibilityChecker();

            // add an invalid box region
            rc.AddInaccessibleRegion(new BoxRegion(1, 1, -1, Pose.Identity));
        }
    }
}
